#include "Plane.h"
#include "Point3D.h"
#include "Segment.h"

#include <cmath>
#include <string>

namespace
  {
  int Sign(double i_x, double i_eps)
    {
    if (i_x > i_eps) return 1;
    if (i_x < i_eps) return -1;
    return 0;
    }
  }

Plane::Plane()
  {
  }

Plane::Plane(const Point3D& i_a, const Point3D& i_b, const Point3D& i_c)
  {
  m_A = (i_b.GetY() - i_a.GetY()) * (i_c.GetZ() - i_a.GetZ()) - (i_b.GetZ() - i_a.GetZ()) * (i_c.GetY() - i_a.GetY());
  m_B = (i_b.GetZ() - i_a.GetZ()) * (i_c.GetX() - i_a.GetX()) - (i_b.GetX() - i_a.GetX()) * (i_c.GetZ() - i_a.GetZ());
  m_C = (i_b.GetX() - i_a.GetX()) * (i_c.GetY() - i_a.GetY()) - (i_b.GetY() - i_a.GetY()) * (i_c.GetX() - i_a.GetX());
  m_D = -i_a.GetX() * m_A - i_a.GetY() * m_B - i_a.GetZ() * m_C;

  double denum = sqrt(m_A * m_A + m_B * m_B + m_C * m_C);
  if (denum > 1e-8)
    {
    m_A /= denum;
    m_B /= denum;
    m_C /= denum;
    m_D /= denum;
    }
  }


Plane::~Plane(void)
  {
  }

double Plane::GetSignDist(const Point3D& i_point) const
  {
  return (m_A * i_point.GetX() + m_B * i_point.GetY() + m_C * i_point.GetZ() + m_D);  
  }

double Plane::GetDist(const Point3D& i_point) const
  {
  return fabs(GetSignDist(i_point));
  }

bool Plane::IsOnPlane(const Point3D& i_point, double i_eps = 1e-8) const
  {
  return GetDist(i_point) < i_eps;  
  }

Point3D Plane::GetIntersectionPoint(const Segment& i_segment, double i_eps = 1e-8) const
  {
  if (IsOnPlane(i_segment.GetA()))
    {
    return i_segment.GetA();
    }
  if (IsOnPlane(i_segment.GetB()))
    {
    return i_segment.GetB();
    }
  double d1 = GetSignDist(i_segment.GetA()), d2 = GetSignDist(i_segment.GetB());
  if (d1 < -i_eps && d2 < -i_eps || d1 > i_eps && d2 > i_eps)
    {
    throw std::string("There are no intersection with plane");
    }
  double l = 0, r = 1;
  //for (int it = 0; it < 82; ++it)
  while ((r - l) > r * i_eps)
    {
    double m = (l + r) / 2;
    double d3 = GetSignDist(i_segment.GetPoint(m));
    if (Sign(d2, i_eps) == Sign(d3, i_eps)) 
      l = m;
    else 
      r = m;
    }
  return i_segment.GetPoint((l + r) / 2);
  }